package oldFiles;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.subsumption.*;
import lejos.nxt.*;
import lejos.util.Timer;
import lejos.util.TimerListener;


class DetectWall implements Behavior, TimerListener 
{
	private int _distance = 100;
	private boolean _suppressed = false;
	private Timer timer;
	
  public DetectWall()
  {
    touch = new TouchSensor(SensorPort.S3);
    sonar = new UltrasonicSensor(SensorPort.S2);
    timer = new Timer(1000,this);
    timer.start();
  }

  public synchronized boolean takeControl()
  {
    return touch.isPressed() || _distance < 25;
  }

  public void suppress()
  {
    _suppressed = true;
  }

  public void action()
  {
    _suppressed = false;
	Motor.A.backward();
	Motor.C.backward();
	try {
		Thread.sleep(1000);
		Motor.A.stop();
		Motor.C.stop();
	} catch (InterruptedException e) {
		// TODO Auto-generated catch block
		
	}
	if (!_suppressed) {
	    Motor.A.rotate(-180, true);// start Motor.A rotating backward
		  Motor.C.rotate(-360, true);  // rotate C farther to make the turn
		  while ((Motor.A.isRotating() || Motor.B.isRotating()) && !_suppressed) {
			  Thread.yield();
		  } 
	}  
	  Motor.A.stop();
	  Motor.C.stop();
  }
  
  private TouchSensor touch;
  private UltrasonicSensor sonar;

	@Override
	public void timedOut() {
		sonar.ping();
	    Sound.pause(20);
	    //Sound.pause(2000);
	    synchronized(this) {_distance = sonar.getDistance();}
	}
}